classdef SimpleDWA
    properties
        robot_radius = 0.3;    % 机器人半径(米)
        max_speed = 1.0;       % 最大线速度(米/秒)
        max_yawrate = 0.5;     % 减小最大角速度(弧度/秒)
        dt = 0.1;              % 时间步长(秒)
        predict_time = 2.0;    % 预测时间(秒)
    end
    
    methods
        function [v, w, best_traj] = plan(obj, pose, goal, obstacles)
            % 确保总是返回有效指令
            v = 0.5; % 默认值
            w = 0;   % 默认值
            best_traj = [];
            
            try
                % 速度采样
                v_samples = linspace(0.2, obj.max_speed, 5);
                w_samples = linspace(-obj.max_yawrate, obj.max_yawrate, 5);
                
                best_score = -inf;
                best_traj = [];
                
                for i = 1:length(v_samples)
                    for j = 1:length(w_samples)
                        v_candidate = v_samples(i);
                        w_candidate = w_samples(j);
                        
                        % 轨迹预测
                        traj = obj.simulate_trajectory(pose, v_candidate, w_candidate);
                        
                        % 计算评分
                        goal_score = obj.calc_goal_score(traj, goal);
                        obstacle_score = obj.calc_obstacle_score(traj, obstacles);
                        speed_score = 0.1 * (v_candidate / obj.max_speed);
                        
                        % 综合评分 
                        total_score = goal_score * 0.6 + obstacle_score * 0.3 + speed_score;
                        
                        % 检查轨迹是否碰撞
                        if obj.check_collision(traj, obstacles)
                            total_score = total_score * 0.1; % 大幅降低碰撞轨迹的分数
                        end
                        
                        % 更新最优轨迹
                        if total_score > best_score
                            best_score = total_score;
                            v = v_candidate;
                            w = w_candidate;
                            best_traj = traj;
                        end
                    end
                end
            catch ME
                fprintf('DWA规划出错: %s\n', ME.message);
                % 返回安全指令
                v = 0.3;
                w = 0;
            end
        end
        
        function traj = simulate_trajectory(obj, pose, v, w)
            % 轨迹预测
            steps = ceil(obj.predict_time / obj.dt);
            traj = zeros(steps, 3);
            
            x = pose(1); y = pose(2); theta = pose(3);
            for i = 1:steps
                % 先更新角度
                theta = theta + w * obj.dt;
                theta = wrapToPi(theta); % 规范化角度
                
                % 再更新位置
                x = x + v * cos(theta) * obj.dt;
                y = y + v * sin(theta) * obj.dt;
                
                traj(i,:) = [x, y, theta];
            end
        end
        
        function score = calc_goal_score(~, traj, goal)
            % 目标方向得分
            if isempty(traj)
                score = 0;
                return;
            end
            dist = norm(traj(end,1:2) - goal);
            score = 1 / (1 + dist); % 距离越小得分越高
        end
        
        function score = calc_obstacle_score(obj, traj, obstacles)
            % 障碍物评分 (修改处5：改进障碍物评分)
            if isempty(obstacles)
                score = 1.0; % 无障碍物时最高分
                return;
            end
            
            min_dist = inf;
            for i = 1:size(traj, 1)
                for j = 1:size(obstacles, 1)
                    dist = norm(traj(i,1:2) - obstacles(j,:));
                    if dist < min_dist
                        min_dist = dist;
                    end
                end
            end
            
            if min_dist < obj.robot_radius
                score = 0; % 碰撞
            elseif min_dist < obj.robot_radius * 2
                score = 0.3; % 太近
            elseif min_dist < obj.robot_radius * 3
                score = 0.7; % 较近
            else
                score = 1.0; % 安全
            end
        end
        
        function collision = check_collision(obj, traj, obstacles)
            % 检查轨迹是否与障碍物碰撞
            collision = false;
            if isempty(obstacles)
                return;
            end
            
            for i = 1:size(traj, 1)
                for j = 1:size(obstacles, 1)
                    dist = norm(traj(i,1:2) - obstacles(j,:));
                    if dist < obj.robot_radius
                        collision = true;
                        return;
                    end
                end
            end
        end
    end
end

%% ========== 角度规范化函数 ==========
function angle = wrapToPi(angle)
    % 将角度规范化到[-π, π]范围
    angle = mod(angle + pi, 2*pi) - pi;
end